/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#ifndef PROTOCOL_H_____
#define PROTOCOL_H_____

//#include "bitestream.h"
#include "gimutils.h"
#include "binbag.h"

//simulation state related protocols
struct TFSRSimObjectRepositionCmd
{ 
  //float Rotation[9];
  float x,y,z;
  float heading;
  
  //bool maintainState;
  
  gim::binbag::CBinBag binbag;
 
  TFSRSimObjectRepositionCmd();
  
  const char * createData(int &size);
  bool parseData(char *data, int size);

};

/******************************************************************************/

//protocol types used with asrobo so they are pretty much obsolete now
/*
struct TFSRSimPositionData
{
  float absoluteX;
  float absoluteY;
  float absoluteZ;
  float absoluteHeading;
  
  float odometryX;
  float odometryY;
  float odometryZ;
  float odometryHeading;
  
  gim::time timeStamp;
  
};

struct TFSRSimMotorCommand
{
  TFSRSimMotorCommand()
    : speed(0.0),
      angularSpeed(0.0),
      acceleration(0.0),
      angularAcceleration(0.0),
      timeStamp() {}
  float speed;
  float angularSpeed;
  float acceleration;
  float angularAcceleration;
  
  gim::time timeStamp;
  
};

struct TFSRSimRangingData
{ 
  int count;
  
  float *distances;
  float *angles;
  
  gim::time timeStamp;
  
  BiteStream *bs;
  bool alloced;
  
  TFSRSimRangingData()
    : count(0),
      distances(NULL),
      angles(NULL),
      timeStamp(),
      bs(NULL),
      alloced(false)
  {    
  }
  
  ~TFSRSimRangingData()
  {
    if (alloced)
      bsFree(bs);    
  }
  
  bool parseData(unsigned char *data)
  {
    BiteStream *b=bsDecode((char*)data);
    if (b==NULL)
      return false;
    
    bsTag *t;
    t=bsNextTag(b);
    count=*((int*)(t->data));
    t=bsNextTag(b);
    distances=(float*)(t->data);  
    t=bsNextTag(b);
    angles=(float*)(t->data);
    t=bsNextTag(b);
    timeStamp=*((gim::time*)(t->data));
    bsFree(b);
    return true;
  }
  unsigned char * createData(int &size)
  {
    if (!alloced)
    {
      bs=bsAlloc();
      alloced=true;
    }
    bsClear(bs);
    bsAddInt(bs,69,count);
    bsAddBin(bs,0,distances,count*sizeof(float));
    bsAddBin(bs,0,angles,count*sizeof(float));
    bsAddBin(bs,0,&timeStamp,sizeof(gim::time));
    bsFinalize(bs);
    
    size=bs->length;
    
    return (unsigned char*)(bs->buf);
  }
  
  private:

  TFSRSimRangingData(const TFSRSimRangingData &)
    : count(0),
      distances(NULL),
      angles(NULL),
      timeStamp(),
      bs(NULL),
      alloced(false) {}
  TFSRSimRangingData &operator =(const TFSRSimRangingData &) { return *this; }
};
*/


#endif

